#ifndef RANSACPLANE_HPP
#define RANSACPLANE_HPP

#include "ransac.hpp"
#include <opencv2/opencv.hpp>

class RansacPlane : public Ransac<cv::Vec3f,cv::Vec4f>
{

public:
    void Setup(const std::vector<cv::Vec3f> &pts, double ap, double et)
    {
        Release();

        nData=pts.size();
        nSelect=3;
        data=new cv::Vec3f[nData];

        for(size_t i=0;i<nData;i++)
        {
            data[i][0]=pts[i][0];
            data[i][1]=pts[i][1];
            data[i][2]=pts[i][2];
        }

        acceptPercentage=ap;
        errorThreshold=et;
    }


    static cv::Vec4f GetPlane(cv::Vec3f p0, cv::Vec3f p1, cv::Vec3f p2)
    {
        cv::Vec3f normal=p0.cross(p1)+p1.cross(p2)+p2.cross(p0);
        normal=normal/cv::norm(normal);
        float d=-normal.ddot(p0);
        return cv::Vec4f(normal[0],normal[1],normal[2],d);
    }


    virtual void ComputeModel(const size_t *indexSelect, cv::Vec4f &modelFit) const
    {
        modelFit=GetPlane(data[indexSelect[0]], data[indexSelect[1]], data[indexSelect[2]]);
    }


    static double DistanceToPlane(cv::Vec3f pt, cv::Vec4f plane)
    {
//        double scale=1/sqrt(modelFit[0]*modelFit[0]+modelFit[1]*modelFit[1]+modelFit[2]*modelFit[2]);
        cv::Vec4f v(pt[0],pt[1],pt[2],1);
        return fabs(v.ddot(plane));
    }

    virtual void ComputeError(double *error, const cv::Vec4f &modelFit) const
    {
//        double scale=1/sqrt(modelFit[0]*modelFit[0]+modelFit[1]*modelFit[1]+modelFit[2]*modelFit[2]);

        for(size_t i=0;i<nData;i++)
        {
            cv::Vec4f v(data[i][0], data[i][1],data[i][2],1);
            double errplane=v.ddot(modelFit);
            error[i]=fabs(errplane);
        }
    }

    static cv::Vec4f BestPlane(const std::vector<cv::Vec3f> &pt)
    {
        cv::Mat xyz=cv::Mat(pt).reshape(1,pt.size());

        cv::Mat xyzmean;
        cv::reduce(xyz, xyzmean, 0, cv::REDUCE_AVG);

        xyz=xyz-cv::repeat(xyzmean, pt.size(), 1);

        cv::Mat w, u, vt;
        cv::SVD::compute(xyz, w, u, vt);

        std::cout<<"w="<<w<<"\n";

        std::cout<<"u="<<u.size()<<"\n";

        std::cout<<"vt="<<vt<<"\n";

        double val[2];
        int idx[2];

        cv::minMaxIdx(w, val, val+1, idx, idx+1);

        cv::Vec3f normal(vt.at<float>(idx[0],0), vt.at<float>(idx[0],1), vt.at<float>(idx[0],2));

        cv::Vec3f xyzm(xyzmean.at<float>(0), xyzmean.at<float>(1), xyzmean.at<float>(2));

        double d=-normal.ddot(xyzm);

        std::cout<<"normal="<<normal<<"\n";

        std::cout<<"xyzmean="<<xyzmean<<"\n";

        std::cout<<"xyzm="<<xyzm<<"\n";

            std::cout.flush();

        return cv::Vec4f(normal[0],normal[1],normal[2],d);

    }
};


#endif // RANSACPLANE_HPP

